﻿#include "CvCalib3D.h"
#include "CvCalibCommon.h"

#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d.hpp>
#include "opencv2/highgui.hpp"
#include "opencv2/imgproc.hpp"

#include <vector>
#include <string>
#include <algorithm>
#include <iostream>
#include <fstream>
#include <iterator>
#include <stdio.h>
#include <stdlib.h>
#include <ctype.h>

using namespace cv;
using namespace std;

CvCalib3D::CvCalib3D()
    : m_imageSize(640, 480), m_boardSize(9,6), m_squareSize(30,30)
    , m_inputFolder("D:/sai/opencv/images/stereo_example/input/")
    , m_outputFolder("D:/sai/opencv/images/stereo_example/output/")
    , m_fileNameFormatL("left%02d.jpg")
    , m_fileNameFormatR("right%02d.jpg")
    , m_fs()
{
}

CvCalib3D::~CvCalib3D()
{
    if (m_fs.isOpened()) {
        m_fs.release();
    }
}

CvCalib3D::CvCalib3D(Size imageSize, Size boardSize, Size squareSize,
                     string inputFolder, string outputFolder,
                     string fileNameFormatL, string fileNameFormatR)
    : m_imageSize(imageSize), m_boardSize(boardSize), m_squareSize(squareSize)
    , m_inputFolder(inputFolder), m_outputFolder(outputFolder)
    , m_fileNameFormatL(fileNameFormatL), m_fileNameFormatR(fileNameFormatR)
    , m_fs()
{
}

void CvCalib3D::getSingleImagePoints(std::vector<std::vector<Point2f> > &imagePoints, int &nimages,
                                         std::vector<string> &imageList, std::vector<string> &goodImageList)
{
    int i, j;
    int imageCount = imageList.size();
    if(imagePoints.size() != (size_t)imageCount) {
        imagePoints.resize((size_t)imageCount);
    }
    for( i = j = 0; i < imageCount; i++ )
    {
        string filename = imageList[i];
        Mat img = imread(m_inputFolder + filename, IMREAD_GRAYSCALE);
        if(img.empty())
            break;
        if( img.size() != m_imageSize ) {
            cout << "The image " << filename << " has the size different from the first image size. Skipping the pair\n";
            break;
        }
        bool found = false;
        vector<Point2f>& corners = imagePoints[j];

        found = findChessboardCorners(img, m_boardSize, corners,
                                      CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE | CALIB_CB_FAST_CHECK);
        if (found) {
            cout << "Found corners in the image " << filename << endl;
            /* 亚像素精确化 */
            cornerSubPix(img, corners, Size(11, 11), Size(-1, -1),
                         TermCriteria(TermCriteria::EPS+TermCriteria::COUNT, 100, 0.0001));

            Mat outImage = img.clone();
            drawChessboardCorners( outImage, m_boardSize, Mat(corners), true );
            imwrite(m_outputFolder + filename, outImage);
        } else {
            cout << "Not found corners in the image " << filename << endl;
            break;
        }

        goodImageList.push_back(m_inputFolder + filename);
        j++;
    }
    cout << j << " images have been successfully detected.\n";
    nimages = j;
}

void CvCalib3D::getDualImagePoints(std::vector<std::vector<Point2f> > imagePoints[], int &nimages, std::vector<std::string> &goodImageList)
{
    int i, j, k;

    for( i = j = 0; i < nimages; i++ )
    {
        for( k = 0; k < 2; k++ )
        {
            char strBuf[20]={0};
            if (k == 0) {   snprintf(strBuf, 20, m_fileNameFormatL.c_str(), i+1); }
            else        {   snprintf(strBuf, 20, m_fileNameFormatR.c_str(), i+1); }
            const string filename(strBuf);
            Mat img = imread(m_inputFolder + filename, IMREAD_GRAYSCALE);
            if(img.empty())
                break;
            if( img.size() != m_imageSize ) {
                cout << "The image " << filename << " has the size different from the first image size. Skipping the pair\n";
                break;
            }
            bool found = false;
            vector<Point2f>& corners = imagePoints[k][j];

            found = findChessboardCorners(img, m_boardSize, corners,
                                          CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE | CALIB_CB_FAST_CHECK);
            if (found) {
                cout << "Found corners in the image " << filename << endl;
                /* 亚像素精确化 */
                cornerSubPix(img, corners, Size(5, 5), Size(-1, -1),
                             TermCriteria(TermCriteria::EPS+TermCriteria::COUNT, 30, 0.01));

                Mat outImage = img.clone();
                drawChessboardCorners( outImage, m_boardSize, Mat(corners), true );
                imwrite(m_outputFolder + filename, outImage);
            } else {
                cout << "Not found corners in the image " << filename << endl;
                break;
            }
        }

        if( k == 2 )
        {
            char strBuf[20]={0};
            snprintf(strBuf, 20, m_fileNameFormatL.c_str(), i+1);
            goodImageList.push_back(m_inputFolder + strBuf);
            snprintf(strBuf, 20, m_fileNameFormatR.c_str(), i+1);
            goodImageList.push_back(m_inputFolder + strBuf);
            j++;
        }
    }
    cout << j << " pairs have been successfully detected.\n";
    nimages = j;
}

void CvCalib3D::stereoCalib(Mat cameraMatrix[2], Mat distCoeffs[2], int &nimages, string &paramsFileName)
{
    ofstream fout(m_outputFolder + paramsFileName + ".txt");  // 保存标定结果的文件
    FileStorage fs(m_outputFolder + paramsFileName + ".yml", FileStorage::WRITE);
    vector<vector<Point2f> > imagePoints[2];   // 左右相机角点
    vector<vector<Point3f> > objectPoints;     // 三维坐标点

    int i, j, k;

    imagePoints[0].resize(nimages);
    imagePoints[1].resize(nimages);
    vector<string> goodImageList;

    cout << "Running get corners of the images ......" << endl;
    // getCalibrationData(imagePoints, nimages, imageSize, boardSize, goodImageList);
    getDualImagePoints(imagePoints, nimages, goodImageList);
    if( nimages < 2 ) {
        cout << "Error: too little pairs to run the calibration\n";
        return;
    }
    cout << "Images count with corners :" << nimages << endl;

    imagePoints[0].resize(nimages);
    imagePoints[1].resize(nimages);
    objectPoints.resize(nimages);

    for( i = 0; i < nimages; i++ )
    {
        for( j = 0; j < m_boardSize.height; j++ )
            for( k = 0; k < m_boardSize.width; k++ )
                objectPoints[i].push_back(Point3f(k*m_squareSize.width, j*m_squareSize.height, 0));
    }

    cout << "Running stereo calibration ...\n";

    // save rotation and translation
    // release fs
    if (fs.isOpened()) {
        fout << "cameraMatrixLeft = " << cameraMatrix[0] << endl;
        fout << "cameraMatrixRight = " << cameraMatrix[1] << endl;
        fs << "cameraMatrixLeft" << cameraMatrix[0];
        fs << "cameraMatrixRight" << cameraMatrix[1];
    }

    Mat R, T, E, F;

    double rms = stereoCalibrate(objectPoints, imagePoints[0], imagePoints[1],
            cameraMatrix[0], distCoeffs[0],
            cameraMatrix[1], distCoeffs[1],
            m_imageSize, R, T, E, F,
            CALIB_FIX_INTRINSIC,
            TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-5) );
    cout << "done with RMS error=" << rms << endl;

    // CALIBRATION QUALITY CHECK
    // because the output fundamental matrix implicitly
    // includes all the output information,
    // we can check the quality of calibration using the
    // epipolar geometry constraint: m2^t*F*m1=0
    double err = 0;
    int npoints = 0;
    vector<Vec3f> lines[2];
    for( i = 0; i < nimages; i++ )
    {
        int npt = (int)imagePoints[0][i].size();
        Mat imgpt[2];
        for( k = 0; k < 2; k++ )
        {
            imgpt[k] = Mat(imagePoints[k][i]);
            undistortPoints(imgpt[k], imgpt[k], cameraMatrix[k], distCoeffs[k], Mat(), cameraMatrix[k]);
            computeCorrespondEpilines(imgpt[k], k+1, F, lines[k]);
        }
        for( j = 0; j < npt; j++ )
        {
            double errij = fabs(imagePoints[0][i][j].x*lines[1][j][0] +
                    imagePoints[0][i][j].y*lines[1][j][1] + lines[1][j][2]) +
                    fabs(imagePoints[1][i][j].x*lines[0][j][0] +
                    imagePoints[1][i][j].y*lines[0][j][1] + lines[0][j][2]);
            err += errij;
        }
        npoints += npt;
    }
    cout << "average epipolar err = " <<  err/npoints << endl;

    cout << "saving intrinsic parameters ..." << endl;
    // save intrinsic parameters
    fout << "M1 = " << cameraMatrix[0] << endl << "D1 = " << distCoeffs[0] << endl << endl <<
            "M2 = " << cameraMatrix[1] << endl << "D2 = " << distCoeffs[1] << endl << endl;
    if (fs.isOpened()) {
        fs << "M1" << cameraMatrix[0] << "D1" << distCoeffs[0] <<
              "M2" << cameraMatrix[1] << "D2" << distCoeffs[1];
        fs << "E" << E << "F" << F;
    }
    Mat R1, R2, P1, P2, Q;
    Rect validRoi[2];

    stereoRectify(cameraMatrix[0], distCoeffs[0],
            cameraMatrix[1], distCoeffs[1],
            m_imageSize, R, T, R1, R2, P1, P2, Q,
            CALIB_ZERO_DISPARITY, 1, m_imageSize, &validRoi[0], &validRoi[1]);

    cout << "saving extrinsic parameters ..." << endl;
    // save extrinsic parameters
    fout << "R = " << R << endl << "T = " << T <<  endl <<
            "R1 = " << R1 << endl << "R2 = " << R2 <<  endl <<
            "P1 = " << P1 <<  endl << "P2 = " << P2 <<  endl << "Q = " << Q  << endl << endl;
    if (fs.isOpened()) {
        fs << "R" << R << "T" << T << "R1" << R1 << "R2" << R2 << "P1" << P1 << "P2" << P2 << "Q" << Q;
    }

    // release fs
    if (fs.isOpened()) {
        fs.release();
    }

    // OpenCV can handle left-right
    // or up-down camera arrangements
    bool isVerticalStereo = fabs(P2.at<double>(1, 3)) > fabs(P2.at<double>(0, 3));

    Mat rmap[2][2];

    //Precompute maps for cv::remap()
    initUndistortRectifyMap(cameraMatrix[0], distCoeffs[0], R1, P1, m_imageSize, CV_16SC2, rmap[0][0], rmap[0][1]);
    initUndistortRectifyMap(cameraMatrix[1], distCoeffs[1], R2, P2, m_imageSize, CV_16SC2, rmap[1][0], rmap[1][1]);

    Mat canvas;
    double sf;
    int w, h;
    sf = 640./MAX(m_imageSize.width, m_imageSize.height);
    w = cvRound(m_imageSize.width*sf);
    h = cvRound(m_imageSize.height*sf);
    canvas.create(h, w*2, CV_8UC3);

    for( i = 0; i < nimages; i++ )
    {
        for( k = 0; k < 2; k++ )
        {
            Mat img = imread(goodImageList[i*2+k], IMREAD_GRAYSCALE), rimg, cimg;
            remap(img, rimg, rmap[k][0], rmap[k][1], INTER_LINEAR);
            cvtColor(rimg, cimg, COLOR_GRAY2BGR);
            Mat canvasPart = !isVerticalStereo ? canvas(Rect(w*k, 0, w, h)) : canvas(Rect(0, h*k, w, h));
            resize(cimg, canvasPart, canvasPart.size(), 0, 0, INTER_AREA);
            if( true )
            {
                Rect vroi(cvRound(validRoi[k].x*sf), cvRound(validRoi[k].y*sf),
                          cvRound(validRoi[k].width*sf), cvRound(validRoi[k].height*sf));
                rectangle(canvasPart, vroi, Scalar(0,0,255), 3, 8);
            }
        }

        for( j = 0; j < canvas.rows; j += 16 )
            line(canvas, Point(0, j), Point(canvas.cols, j), Scalar(0, 255, 0), 1, 8);

        if (true) {
            char outFileName[20] = {0};
            snprintf(outFileName, 20, "rectfified%02d.jpg", i+1);
            imwrite(m_outputFolder + "rectified/" + outFileName, canvas);
        } else  {
            imshow("rectified", canvas);
            char c = (char)waitKey();
            if( c == 27 || c == 'q' || c == 'Q' )
                break;
        }
    }
}

void CvCalib3D::stereoCalib(int &nimages, string &paramsFileName)
{
    ofstream fout(m_outputFolder + paramsFileName + ".txt");  // 保存标定结果的文件
    FileStorage fs(m_outputFolder + paramsFileName + ".yml", FileStorage::WRITE);
    vector<vector<Point2f> > imagePoints[2];   // 左右相机角点
    vector<vector<Point3f> > objectPoints;     // 三维坐标点

    int i, j, k;

    imagePoints[0].resize(nimages);
    imagePoints[1].resize(nimages);
    vector<string> goodImageList;

    cout << "Running get corners of the images ......" << endl;
    // getDualImagePoints(imagePoints, nimages, imageSize, boardSize, goodImageList);
    getDualImagePoints(imagePoints, nimages, goodImageList);
    if( nimages < 2 ) {
        cout << "Error: too little pairs to run the calibration\n";
        return;
    }
    cout << "Images count with corners :" << nimages << endl;

    imagePoints[0].resize(nimages);
    imagePoints[1].resize(nimages);
    objectPoints.resize(nimages);

    for( i = 0; i < nimages; i++ )
    {
        for( j = 0; j < m_boardSize.height; j++ )
            for( k = 0; k < m_boardSize.width; k++ )
                objectPoints[i].push_back(Point3f(k*m_squareSize.width, j*m_squareSize.height, 0));
    }

    cout << "Running stereo calibration ...\n";

    Mat cameraMatrix[2] = {Mat(3,3,CV_32FC1,Scalar::all(0)), Mat(3,3,CV_32FC1,Scalar::all(0))}; // 摄像机内参数矩阵
    Mat distCoeffs[2] = {Mat(1,5,CV_32FC1,Scalar::all(0))}; // 摄像机的5个畸变系数：k1,k2,p1,p2,k3
    //cameraMatrix[0] = initCameraMatrix2D(objectPoints,imagePoints[0],imageSize,0);
    //cameraMatrix[1] = initCameraMatrix2D(objectPoints,imagePoints[1],imageSize,0);
    vector<cv::Mat> rotationVectors[2];       /* 每幅图像的旋转向量 */
    vector<cv::Mat> translationVectors[2];    /* 每幅图像的平移向量 */
    calibrateCamera(objectPoints, imagePoints[0], m_imageSize, cameraMatrix[0], distCoeffs[0],
            rotationVectors[0], translationVectors[0], 0);
    calibrateCamera(objectPoints, imagePoints[1], m_imageSize, cameraMatrix[1], distCoeffs[1],
            rotationVectors[1], translationVectors[1], 0);

    // save rotation and translation
    // release fs
    if (fs.isOpened()) {
        fout << "cameraMatrixLeft = " << cameraMatrix[0] << endl;
        fout << "cameraMatrixRight = " << cameraMatrix[1] << endl;
        fs << "cameraMatrixLeft" << cameraMatrix[0];
        fs << "cameraMatrixRight" << cameraMatrix[1];

        Mat rotationMatrix[2] = {Mat(3,3,CV_32FC1, Scalar::all(0)), Mat(3,3,CV_32FC1, Scalar::all(0))}; /* 保存每幅图像的旋转矩阵 */
        for (i = 0; i < nimages; i++) {
            /* 将旋转向量转换为相对应的旋转矩阵 */
            Rodrigues(rotationVectors[0][i],rotationMatrix[0]);
            Rodrigues(rotationVectors[1][i],rotationMatrix[1]);
            fout << "rotationVectorLeft_" << i+1 << " = " << rotationVectors[0][i] << endl;
            fout << "rotationMatrixLeft_" << i+1 << " = " << rotationMatrix[0] << endl;
            fout << "translationVectorLeft_" << i+1 << " = " << translationVectors[0][i] << endl;
            fout << "rotationVectorRight_" << i+1 << " = " << rotationVectors[1][i] << endl;
            fout << "rotationMatrixRight_" << i+1 << " = " << rotationMatrix[1] << endl;
            fout << "translationVectorRight_" << i+1 << " = " << translationVectors[1][i] << endl;

            string indexStr = to_string(i+1);
            fs << "rotationVectorLeft_" + indexStr << rotationVectors[0][i];
            fs << "rotationMatrixLeft_"  + indexStr  << rotationMatrix[0] ;
            fs << "translationVectorLeft_"  + indexStr  << translationVectors[0][i] ;
            fs << "rotationVectorRight_"  + indexStr  << rotationVectors[1][i] ;
            fs << "rotationMatrixRight_"  + indexStr  << rotationMatrix[1];
            fs << "translationVectorRight_"  + indexStr << translationVectors[1][i];
        }
    }

    Mat R, T, E, F;

    double rms = stereoCalibrate(objectPoints, imagePoints[0], imagePoints[1],
            cameraMatrix[0], distCoeffs[0],
            cameraMatrix[1], distCoeffs[1],
            m_imageSize, R, T, E, F,
            CALIB_FIX_INTRINSIC,
            TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-5) );
    cout << "done with RMS error=" << rms << endl;

    // CALIBRATION QUALITY CHECK
    // because the output fundamental matrix implicitly
    // includes all the output information,
    // we can check the quality of calibration using the
    // epipolar geometry constraint: m2^t*F*m1=0
    double err = 0;
    int npoints = 0;
    vector<Vec3f> lines[2];
    for( i = 0; i < nimages; i++ )
    {
        int npt = (int)imagePoints[0][i].size();
        Mat imgpt[2];
        for( k = 0; k < 2; k++ )
        {
            imgpt[k] = Mat(imagePoints[k][i]);
            undistortPoints(imgpt[k], imgpt[k], cameraMatrix[k], distCoeffs[k], Mat(), cameraMatrix[k]);
            computeCorrespondEpilines(imgpt[k], k+1, F, lines[k]);
        }
        for( j = 0; j < npt; j++ )
        {
            double errij = fabs(imagePoints[0][i][j].x*lines[1][j][0] +
                    imagePoints[0][i][j].y*lines[1][j][1] + lines[1][j][2]) +
                    fabs(imagePoints[1][i][j].x*lines[0][j][0] +
                    imagePoints[1][i][j].y*lines[0][j][1] + lines[0][j][2]);
            err += errij;
        }
        npoints += npt;
    }
    cout << "average epipolar err = " <<  err/npoints << endl;

    cout << "saving intrinsic parameters ..." << endl;
    // save intrinsic parameters
    fout << "M1 = " << cameraMatrix[0] << endl << "D1 = " << distCoeffs[0] << endl << endl <<
            "M2 = " << cameraMatrix[1] << endl << "D2 = " << distCoeffs[1] << endl << endl;
    if (fs.isOpened()) {
        fs << "M1" << cameraMatrix[0] << "D1" << distCoeffs[0] <<
              "M2" << cameraMatrix[1] << "D2" << distCoeffs[1];
        fs << "E" << E << "F" << F;
    }
    Mat R1, R2, P1, P2, Q;
    Rect validRoi[2];

    stereoRectify(cameraMatrix[0], distCoeffs[0],
            cameraMatrix[1], distCoeffs[1],
            m_imageSize, R, T, R1, R2, P1, P2, Q,
            CALIB_ZERO_DISPARITY, 1, m_imageSize, &validRoi[0], &validRoi[1]);

    cout << "saving extrinsic parameters ..." << endl;
    // save extrinsic parameters
    fout << "R = " << R << endl << "T = " << T <<  endl <<
            "R1 = " << R1 << endl << "R2 = " << R2 <<  endl <<
            "P1 = " << P1 <<  endl << "P2 = " << P2 <<  endl << "Q = " << Q  << endl << endl;
    if (fs.isOpened()) {
        fs << "R" << R << "T" << T << "R1" << R1 << "R2" << R2 << "P1" << P1 << "P2" << P2 << "Q" << Q;
    }

    // release fs
    if (fs.isOpened()) {
        fs.release();
    }

    // OpenCV can handle left-right
    // or up-down camera arrangements
    bool isVerticalStereo = fabs(P2.at<double>(1, 3)) > fabs(P2.at<double>(0, 3));

    Mat rmap[2][2];

    //Precompute maps for cv::remap()
    initUndistortRectifyMap(cameraMatrix[0], distCoeffs[0], R1, P1, m_imageSize, CV_16SC2, rmap[0][0], rmap[0][1]);
    initUndistortRectifyMap(cameraMatrix[1], distCoeffs[1], R2, P2, m_imageSize, CV_16SC2, rmap[1][0], rmap[1][1]);

    Mat canvas;
    double sf;
    int w, h;
    sf = 600./MAX(m_imageSize.width, m_imageSize.height);
    w = cvRound(m_imageSize.width*sf);
    h = cvRound(m_imageSize.height*sf);
    canvas.create(h, w*2, CV_8UC3);

    for( i = 0; i < nimages; i++ )
    {
        for( k = 0; k < 2; k++ )
        {
            Mat img = imread(goodImageList[i*2+k], IMREAD_GRAYSCALE), rimg, cimg;
            remap(img, rimg, rmap[k][0], rmap[k][1], INTER_LINEAR);
            cvtColor(rimg, cimg, COLOR_GRAY2BGR);
            Mat canvasPart = !isVerticalStereo ? canvas(Rect(w*k, 0, w, h)) : canvas(Rect(0, h*k, w, h));
            resize(cimg, canvasPart, canvasPart.size(), 0, 0, INTER_AREA);
            if( true )
            {
                Rect vroi(cvRound(validRoi[k].x*sf), cvRound(validRoi[k].y*sf),
                          cvRound(validRoi[k].width*sf), cvRound(validRoi[k].height*sf));
                rectangle(canvasPart, vroi, Scalar(0,0,255), 3, 8);
            }
        }

        for( j = 0; j < canvas.rows; j += 16 )
            line(canvas, Point(0, j), Point(canvas.cols, j), Scalar(0, 255, 0), 1, 8);

        if (true) {
            char outFileName[20] = {0};
            snprintf(outFileName, 20, "rectfified%02d.jpg", i+1);
            imwrite(m_outputFolder + "rectified/" + outFileName, canvas);
        } else  {
            imshow("rectified", canvas);
            char c = (char)waitKey();
            if( c == 27 || c == 'q' || c == 'Q' )
                break;
        }
    }
}

void CvCalib3D::readParamsFromFiles(std::string filename)
{
    if (!m_fs.isOpened()) {
        m_fs.open(m_outputFolder + filename,  FileStorage::READ);
    }
    m_fs["M1"] >> M1;
    m_fs["D1"] >> D1;
    m_fs["M2"] >> M2;
    m_fs["D2"] >> D2;

    m_fs["R"] >> R;
    m_fs["T"] >> T;

    cout << "M1=" << M1 << endl;
    cout << "D1=" << D1 << endl;
    cout << "M2=" << M2 << endl;
    cout << "D2=" << D2 << endl;
}

void CvCalib3D::verifyImagePointsTo3D(Mat &imageL, Mat &imageR, string outPointsFileName,
                            double &err1, double &err2)
{
    int i;
    // 定义 单位旋转矩阵和0平移矩阵
    Mat R0 = Mat::eye(3,3, CV_64FC1);
    Mat T0 =Mat(3,1,CV_64FC1,Scalar::all(0));

    // 1. 读取原始图像, 如果不是灰度，药转成灰度图像
    Mat img1 = imageL;
    Mat img2 = imageR;

    // 2. 矫正图像
    Mat map1x, map1y, map2x, map2y;
    // initUndistortRectifyMap(M1, D1, R, M1, imageSize, CV_32FC1, map1x, map1y);
    initUndistortRectifyMap(M1, D1, R0, M1, m_imageSize, CV_16SC2, map1x, map1y);
    initUndistortRectifyMap(M2, D2, R0, M2, m_imageSize, CV_16SC2, map2x, map2y);

    Mat img1R = img1.clone();
    Mat img2R = img2.clone();
    cv::remap( img1, img1R, map1x, map1y, INTER_LINEAR);
    cv::remap( img2, img2R, map2x, map2y, INTER_LINEAR);

    // 3. 记录矫正的结果
    imwrite(m_outputFolder + "left01_r.jpg", img1R);
    imwrite(m_outputFolder + "right01_r.jpg", img2R);

    // 4. 获取角点
    vector<Point2f> corners1, corners1R;            // 缓存每幅图像上检测到的角点
    vector<Point2f> corners2, corners2R;            // 缓存每幅图像上检测到的角点

    bool found1 = CvCalibCommon::findChessboardCorners(img1, m_boardSize, corners1);
    bool found2 = CvCalibCommon::findChessboardCorners(img1R, m_boardSize, corners1R);
    if (!found1 || !found2) {
        cout << "Not found corners(Left)!" << endl;
        return;
    }

    found1 = CvCalibCommon::findChessboardCorners(img2, m_boardSize, corners2);
    found2 = CvCalibCommon::findChessboardCorners(img2R, m_boardSize, corners2R);
    if (!found1 || !found2) {
        cout << "Not found corners!" << endl;
        return;
    }


    // 5. 计算三维坐标
    vector<Point3f> pointsWorld;
    for (i=0; i< corners1R.size();i++) {
        Point3f worldPoint = CvCalibCommon::uv2xyz(corners1R[i], corners2R[i], M1, R0, T0,
                                   M2, R, T);
        //cout << i << ": " << corners1[i] << ", " << corners2R[i] ;
        //cout << " worldPoint: " << worldPoint << endl;
        pointsWorld.push_back(worldPoint);
    }

    // !!!save to file
    string filename = m_outputFolder + outPointsFileName;
    ofstream fpoint(filename);

    for (i=0; i< corners1.size(); i++) {
        fpoint << i+1 << ": " << corners1[i] << ", " << corners2R[i] ;
        fpoint << " worldPoint: " << pointsWorld[i] << endl;
    }

    // 6. 计算两两点的距离
    cout << "compute distances adjected points" << endl;
    fpoint << "distances:";
    for (i=0; i < pointsWorld.size() - m_boardSize.width - 1;i++) {
        Point3f point1 = pointsWorld[i+1] - pointsWorld[i];
        Point3f point2 = pointsWorld[i+m_boardSize.width] - pointsWorld[i];
        double d1 = sqrt(point1.ddot(point1));
        double d2 = sqrt(point2.ddot(point2));
        if (i%m_boardSize.width == 0) {
            fpoint << endl;
            fpoint << i+1 << ": ";
        }
        fpoint << " [" << d1 << "," << d2 << "]";
    }
    fpoint << endl;


    // 7. 对计算结果进行评价
    {
        cout << "estimating the result ..." << endl;
        vector<Point2f>  imagePoints1, imagePoints2; // 保存重新计算得到的投影点
        // double err1 = 0.0, err2 = 0.0;                            // 每幅图像的平均误差
        /* 通过得到的摄像机内外参数，对空间的三维点进行重新投影计算，得到新的投影点 */
        projectPoints(pointsWorld, R0, T0, M1, D1, imagePoints1);
        projectPoints(pointsWorld, R, T, M2, D2, imagePoints2);
        Mat corners1Mat = Mat(1,pointsWorld.size(),CV_32FC2);
        Mat corners2Mat = Mat(1,pointsWorld.size(),CV_32FC2);
        Mat imagePoints1mat = Mat(1,imagePoints1.size(), CV_32FC2);
        Mat imagePoints2mat = Mat(1,imagePoints2.size(), CV_32FC2);
        for (size_t j = 0 ; j != pointsWorld.size(); j++)
        {
            corners1Mat.at<Vec2f>(0,j) = Vec2f(corners1[j].x, corners1[j].y);
            corners2Mat.at<Vec2f>(0,j) = Vec2f(corners2[j].x, corners2[j].y);

            imagePoints1mat.at<Vec2f>(0,j) = Vec2f(imagePoints1[j].x, imagePoints1[j].y);
            imagePoints2mat.at<Vec2f>(0,j) = Vec2f(imagePoints2[j].x, imagePoints2[j].y);
        }
        err1 = norm(imagePoints1mat, corners1Mat, NORM_L2);
        err2 = norm(imagePoints2mat, corners2Mat, NORM_L2);
        fpoint << "imagePoints1mat = " << imagePoints1mat << endl;
        fpoint << "corners1Mat = " << corners1Mat << endl;
        fpoint << "imagePoints2mat = " << imagePoints2mat << endl;
        fpoint << "corners2Mat = " << corners2Mat << endl;
        fpoint << "img1 err = " << err1 << endl;
        fpoint << "img2 err = " << err2 << endl;
        cout << "img1 err = " << err1 << endl;
        cout << "img2 err = " << err2 << endl;
    }

    // !!! close file
    fpoint.close();

}

Point3f CvCalib3D::uv2xyz(Point2f &uvLeft, Point2f &uvRight)
{
    // 定义 单位旋转矩阵和0平移矩阵
    Mat R0 = Mat::eye(3,3, CV_64FC1);
    Mat T0 =Mat(3,1,CV_64FC1,Scalar::all(0));
    return CvCalibCommon::uv2xyz(uvLeft, uvRight, M1, R0, T0,
                                 M2, R, T);
}

